COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00004 00003	⊃ PROCEDURES FOR GENERATING CODE OF THE PDP-11
C00007 00004	INTERNAL PROCEDURE MOVE(RPTR(FRAME)F_ARM,DEST REAL TT(2.0))
C00010 00005	INTERNAL PROCEDURE DRIVE(INTEGER ARM,J,ABSREL REAL AMNT,TT(2.0))
C00012 00006	INTERNAL PROCEDURE CENTER(INTEGER ARM)
C00013 ENDMK
C⊗;
ENTRY;
BEGIN "MOVE  - GENERATES SIMPLE TRAJECTORY AND SENDS DATA TO BLUE ARM"
COMMENT ALL [PNT,HE] FILES COPIED OVER FROM [PNT,MSM] DEC 30,1977 ;
DEFINE ⊃="COMMENT";

DEFINE $MOVARM=TRUE;

REQUIRE "HEADER.SAI" SOURCE_FILE;
REQUIRE "MOVE.DEF[PNT,HE]" SOURCE_FILE;

INTEGER SEGS,TNUM,JTS,I,J,K;
INTEGER ARRAY DATA[1:1000];
INTEGER DUM, PTR;
REAL ARRAY DD[1:20],OLD[1:7],NEW[1:7];
! INTERNAL REAL BHAND;


DEFINE	MASTER="'54321";
DEFINE  MOVE_CODE= "'76";
DEFINE  CENTER_CODE= "'67";


⊃ PROCEDURES FOR GENERATING CODE OF THE PDP-11;

SIMPLE PROCEDURE INTOUT(INTEGER NUM);
	BEGIN
	DATA[PTR]←NUM;
	PTR←PTR+1;
	END;




⊃ PROCEDURE FOR SAVING A FLOATING POINT NUMBER IN 11 FORMAT IN THE DATA ARRAY;

SIMPLE PROCEDURE FLTOUT(REAL FNUM);
	BEGIN
	LABEL ST1,ST2,OVER,FLTEND;
	INTEGER BYTE,NUM1,NUM2;
	BYTE←'013200000002;
		START_CODE
		   	MOVE   2,FNUM;
			JUMPGE 2,ST1;
			MOVN   2,2;
 			TLO    2,'400000;
		ST1:	JFCL   2,ST2;
		ST2:	ADDI   2,4;
			JFCL   2,OVER;
     		    	DPB    2,BYTE;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM1;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM2;
		END;
	DATA[PTR]←NUM1;
	PTR←PTR+1;
	DATA[PTR]←NUM2;
	PTR←PTR+1;
	GOTO FLTEND;
OVER:	OUTSTR("ERROR-ROUNDING OVERFLOW"&CRLF);
FLTEND:	END; 
INTERNAL PROCEDURE MOVE(RPTR(FRAME)F_ARM,DEST; REAL TT(2.0));
BEGIN "MOVE"
	INTEGER JTS,TIME,SEGPTR,JOINT,FLAG;
	INTEGER MECH,BITS,ARM;
	REAL DIF; REAL ARRAY DIFA[1:6];

	IF F_ARM=F_BARM
		THEN BEGIN DUM←0; ARM←BLUE; MECH←BARM_MECH; BITS←BARMSB; END
		ELSE BEGIN DUM←1; ARM←YELLOW;MECH←YARM_MECH; BITS←YARMSB; END;

	READ_ARM(ARM); ARRTRAN(NEW,BANGLE);
⊃ SET UP THE COEFFICIENT LIST HEADER;
	SETFORMAT(10,3);
	JTS←6;
	SEGPTR←8+JTS*32;
	PTR←1;
⊃ ***** ; INTOUT(MOVE_CODE);
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0); ⊃ WOBBLE;

	FLAG←ARMSOL(ARM,NEW,DEST); ⊃ GIVES THE ANGLES OF THE NEW TRANSFORM;

	IF FLAG>0 THEN
		BEGIN
		STRING S; INTEGER I;
		S←CRLF&"CANT MOVE THERE. FOLLOWING JOINT(S) OUT OF RANGE: ";
		FOR I←1 STEP 1 UNTIL 7
			DO IF FLAG LAND (2↑(I-1)) THEN  S← S&"  "&CVS(I)&",";
		IF FLAG LAND '200 THEN ABORT1(CRLF&"TRYING TO PUSH ARM THROUGH TABLE");
		ABORT1(S[1 TO ∞ - 1]);
		END;

	TIME←TT*1000;
	FOR J←1 STEP 1 UNTIL JTS DO BEGIN
		DIFA[J]←NEW[J] - OLD[J];
		TIME← TIME MAX (1000 + ABS(DIFA[J])*TIMFAC[ARM,J]/30*1000);
		END;
			INTOUT(SEGPTR);
			INTOUT(TIME);
			INTOUT(0); INTOUT(0);

⊃ WRITE OUT THE POLYNOMIAL AND DYNAMIC COEFFICIENTS;

			FOR J←1 STEP 1 UNTIL JTS DO BEGIN
				JOINT←J;
				DIF←DIFA[JOINT];
				FLTOUT(OLD[JOINT]);
				FLTOUT(0.0);  FLTOUT(0.0);
				FLTOUT(10.0*DIF);
				FLTOUT(-15.0*DIF);
				FLTOUT(6.0*DIF);
			END;
!   ***************	DTERMS(DD,NEW[1],DUM);
			DTERMS(DD,NEW[1],MECH);
			FOR J←1 STEP 1 UNTIL JTS DO BEGIN
				K←(J-1)*2+1;
				FLTOUT(DD[K]);
				FLTOUT(DD[K+1]);
			END;
			INTOUT(0);

		TLKEF3(MASTER,DATA); ⊃ tell to move;

END "MOVE";

	PRESET_WITH '100000,'40000,'20000,'10000,'4000,'2000,'1000,
			'400,'200,'100,'40,'20,'10,'4;
	INTEGER ARRAY JT_CODE[0:1,1:7];
INTERNAL PROCEDURE DRIVE(INTEGER ARM,J,ABSREL; REAL AMNT,TT(2.0));
BEGIN "DRIVE"
	⊃ DRIVES ONE JOINT;
	INTEGER JTS,SEGPTR,TIME,BITS,K1;
	REAL DIF, OLDJ,NEWJ;
	DUM ←IF J=7 THEN IF ARM=BLUE THEN BHAND_MECH ELSE YHAND_MECH
			ELSE IF ARM=BLUE THEN BARM_MECH ELSE YARM_MECH;

	BITS←JT_CODE[ARM,J];
	READ_ARM(ARM); ARRTRAN(OLD,BANGLE);
	OLDJ←OLD[J];
	IF ABSREL = ABS_MOTION THEN DIF←(NEWJ←AMNT)- OLDJ
		ELSE NEWJ←OLDJ+(DIF←AMNT);
	IF ¬(LOSTOP[ARM,J]≤NEWJ≤HISTOP[ARM,J]) THEN
		ABORT1(CRLF&"TRYING TO DRIVE JT["&CVS(J)&"] TO "&
				CVF(NEWJ)&" WHICH IS OUTSIDE JOINT LIMITS "&
				CVF(LOSTOP[ARM,J])&" AND "&CVF(HISTOP[ARM,J]));
	JTS←1; PTR←1; SEGPTR←8+JTS*32;
	SETFORMAT(10,3);
	INTOUT(MOVE_CODE);
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0);
	TIME←(TT MAX (ABS(TIMFAC[ARM,J]*DIF/30.0)+1))*1000;
	INTOUT(SEGPTR);
	INTOUT(TIME);
	INTOUT(0);
	INTOUT(0);
	FLTOUT(OLDJ);
	FLTOUT(0.0);FLTOUT(0.0);
	FLTOUT(10.0*DIF);
	FLTOUT(-15.0*DIF);
	FLTOUT(6.0*DIF);
	FOR K1←1 STEP 1 UNTIL 7 DO NEW[K1]←OLD[K1];
	NEW[J]←NEWJ;
	DTERMS(DD,NEW[1],DUM);
	IF J=7 THEN K←1 ELSE K←(J -1)*2 +1;
	FLTOUT(DD[K]);
	FLTOUT(DD[K+1]);
	INTOUT(0);
	TLKEF3(MASTER,DATA); ⊃ TELL TO MOVE;
END "DRIVE";
INTERNAL PROCEDURE CENTER(INTEGER ARM);
BEGIN "CENTER"
	INTEGER BITS;
	IF ARM=BLUE THEN BITS←(BARMSB+BHANDSB) ELSE BITS←(YHANDSB+YARMSB);
	PTR←1;
	INTOUT(CENTER_CODE);
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0);	⊃ NO WOBBLE;
	INTOUT(0);	⊃ NO NEXT SEGMENT;
	INTOUT(0);	⊃ NO FUNCTION TIME;
	INTOUT(0);	⊃ NO TRANSFORM;
	INTOUT(0);
	INTOUT(0);
	TLKEF3(MASTER,DATA);
END "CENTER";


END;